/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Jaguar;
import com.sun.squawk.VM;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.Watchdog;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class EncoderCalibration extends IterativeRobot {

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    Joystick leftStick;
    Jaguar left, right;
    Encoder leftEncoder, rightEncoder;
    DriverStation DS;
    Dashboard dashboard;
    DriverStationLCD dslcd;

    public void robotInit() {
	  left = new Jaguar(4, 1);
	  right = new Jaguar(4, 2);
	  leftStick = new Joystick(1);
	  leftEncoder = new Encoder(1,2);
	  rightEncoder = new Encoder(3,4, true);
	  DS = DriverStation.getInstance();
	  dashboard = DS.getDashboardPackerLow();
	  dslcd = DriverStationLCD.getInstance();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
    }

    /**
     * This function is called periodically during operator control
     */
    //right 1&2
    //left 3&4

    public void teleopPeriodic() {
	  leftEncoder.start();
	  leftEncoder.reset();
	  rightEncoder.start();
	  rightEncoder.reset();
	  long counter=0;
	  while (isOperatorControl()) {
		if (isEnabled()) {
		    if (leftStick.getRawButton(1)) {
			  double leftY = leftStick.getY();
			  left.set(leftY);
			  right.set(-leftY);
		    } else if(leftStick.getRawButton(7)){
			  left.set(leftStick.getRawAxis(3)-.2);
			  right.set(-leftStick.getRawAxis(3));
		    } else {
			  left.set(0);
			  right.set(0);
		    }
		    if (leftStick.getRawButton(11)){
			  leftEncoder.reset();
			  rightEncoder.reset();
		    }
		    if (leftStick.getRawButton(2)) {
			  /*dashboard.addCluster();
			  dashboard.addString("Lenc:" + leftEncoder.get() + "\t" + "Renc:" + rightEncoder.get());
			  dashboard.finalizeCluster();
			  dashboard.commit();
			   * */
			  VM.println("Lenc:" + leftEncoder.get() + "\t" + "Renc:" + rightEncoder.get());
		    }
		    if(counter>=500){
			  dslcd.println(DriverStationLCD.Line.kUser5, 1, "LE:" + leftEncoder.get() + "   " + "RE:" + rightEncoder.get()+"  ");
			  dslcd.updateLCD();
			  counter=0;
		    }
		    else counter++;

		} else {
		    left.set(0);
		    right.set(0);
		}
		Watchdog.getInstance().feed();
	  }
	  leftEncoder.stop();
	  rightEncoder.stop();
    }
     /*void updateDashboard() {
        Dashboard lowDashData = DriverStation.getInstance().getDashboardPackerLow();
        lowDashData.addCluster();
        {
            lowDashData.addCluster();
            {     //analog modules
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(1).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
                lowDashData.addCluster();
                {
                    for (int i = 1; i <= 8; i++) {
                        lowDashData.addFloat((float) AnalogModule.getInstance(2).getAverageVoltage(i));
                    }
                }
                lowDashData.finalizeCluster();
            }
            lowDashData.finalizeCluster();

            lowDashData.addCluster();
            { //digital modules
                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 4;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

                lowDashData.addCluster();
                {
                    lowDashData.addCluster();
                    {
                        int module = 6;
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayForward());
                        lowDashData.addByte(DigitalModule.getInstance(module).getRelayReverse());
                        lowDashData.addShort(DigitalModule.getInstance(module).getAllDIO());
                        lowDashData.addShort(DigitalModule.getInstance(module).getDIODirection());
                        lowDashData.addCluster();
                        {
                            for (int i = 1; i <= 10; i++) {
                                lowDashData.addByte((byte) DigitalModule.getInstance(module).getPWM(i));
                            }
                        }
                        lowDashData.finalizeCluster();
                    }
                    lowDashData.finalizeCluster();
                }
                lowDashData.finalizeCluster();

            }
            lowDashData.finalizeCluster();

            lowDashData.addByte(Solenoid.getAll());
        }
        lowDashData.finalizeCluster();
        lowDashData.commit();

    }//*/
}
